#ifndef H_SERVO
#define H_SERVO

#include "Arduino.h"

volatile byte highState[2],lowState[2];

class Servo
{
private:

public:
  void setup()
  {
#ifdef SERVO
    TCCR0A = 0; // normal counting mode TODO check if have some problem with timing
    TIMSK0 |= (1<<OCIE0A); // Enable CTC interrupt
    TIMSK0 |= (1<<OCIE0B);
#if !defined(A0_A1_PIN_HEX) 
    pinMode(5,OUTPUT);
    pinMode(6,OUTPUT);
#else
    pinMode(A0,OUTPUT);
    pinMode(A1,OUTPUT);
#endif
#endif
  }

  void analogWrite(byte ch,byte us8) // channel 0 or 1, in 8 microseconds
  {
    highState[ch] = us8;
    lowState[ch] = 255-highState[ch];
  }


  void update()
  {
    this->analogWrite(0,esc.ichannelOut[4]>>3);
    this->analogWrite(1,esc.ichannelOut[5]>>3);
  }
} 
servo;

#ifdef SERVO
ISR(TIMER0_COMPA_vect) {
  static uint8_t state = 0;
  if (state == 0) {
#if not defined(A0_A1_PIN_HEX)
    PORTD |= 1<<5; //digital PIN 5 high
#else
    PORTC |= 1<<0;//PIN A0
#endif
    OCR0A+= highState[0]; //250 x 4 microsecons = 1ms
    state = 1;
  } 
  else if (state == 1) {
    OCR0A+= highState[0];
    state = 2;
  } 
  else if (state == 2) {
#if not defined(A0_A1_PIN_HEX)
    PORTD &= ~(1<<5); //digital PIN 5 low
#else
    PORTC &= ~(1<<0);
#endif 
    OCR0A+= lowState[0];
    state = 3;
  } 
  else if (state >= 3) {
#ifdef SERVOFAST
    OCR0A+= lowState[0];
    state = 0;
#else
    OCR0A+= 250; // 1 ms
    if (state++>=20) // total 20ms
      state = 0;
#endif
  }
}

ISR(TIMER0_COMPB_vect) {
  static uint8_t state = 0;
  if (state == 0) {
#if not defined(A0_A1_PIN_HEX)
    PORTD |= 1<<6; //digital PIN 6 high
#else
    PORTC |= 1<<1;//PIN A1
#endif
    OCR0B+= highState[1]; //250 x 4 microsecons = 1ms
    state = 1;
  } 
  else if (state == 1) {
    OCR0B+= highState[1];
    state = 2;
  } 
  else if (state == 2) {
#if not defined(A0_A1_PIN_HEX)
    PORTD &= ~(1<<6); //digital PIN 6 low
#else
    PORTC &= ~(1<<1); 
#endif 
    OCR0B+= lowState[1];
    state = 3;
  } 
  else if (state >= 3) {
#ifdef SERVOFAST
    OCR0B+= lowState[1];
    state = 0;
#else
    OCR0B+= 250; // 1 ms
    if (state++>=20) // total 20ms
      state = 0;
#endif
  }
}

#endif
#endif







